#region Namespaces
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Text;
using System.Drawing;
using System.Drawing.Imaging;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;
using System.Xml;
using xna = Microsoft.Xna.Framework;
using xnagrfx = Microsoft.Xna.Framework.Graphics;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using simengine = Microsoft.Robotics.Simulation.Engine.Proxy;
#region Real namespaces
using webcam = Microsoft.Robotics.Services.WebCam.Proxy;
using lrf = Microsoft.Robotics.Services.Sensors.SickLRF.Proxy;
#endregion
#region Simulation namespaces
using Microsoft.Robotics.Simulation;
using Microsoft.Robotics.Simulation.Engine;
using engineproxy = Microsoft.Robotics.Simulation.Engine.Proxy;
using Microsoft.Robotics.Simulation.Physics;
using Microsoft.Robotics.PhysicalModel;
using simlrf = Microsoft.Robotics.Services.Simulation.Sensors.LaserRangeFinder.Proxy;
using simwebcam = Microsoft.Robotics.Services.Simulation.Sensors.SimulatedWebcam.Proxy;
#endregion

#endregion

namespace SimJaguars
{
    [Contract(Contract.Identifier)]
    [DisplayName("SimJaguars")]
    [Description("SimJaguars service same as SimJaguar but with 3 entities")]
    class SimJaguarsService : DsspServiceBase
    {
        #region Service Properties
        //Robot entity variables for simulation aquisition of pose and orientation.
        //If any additional robot, a set of handlers for the robot must be created !!
        public const int num_of_robots = 3; // The number of robots
        JaguarEntity[] _jaguar = new JaguarEntity[num_of_robots + 1]; //The diffdrives ignore index 0
        System.Diagnostics.Stopwatch simtime = new System.Diagnostics.Stopwatch();  // For knowing simulation time

        //As the readings from partners are obtained by static linkage, this simplifies when changing from computer.
        //If additional changes, refer to method InsertEntityNotificationHandler.
        public const string localhost = "jaguar1-erobots";
        public const string tcpport = "50001";

        //According to PROMRDS AddMaze requires from a file, which is inside Store\Media\Maze_Textures, so if the file
        //is saved in the same path, just write here the name and it will be loaded. Leaving it blank uses ModelLarge.bmp.
        string Mazefile = "usarmaze_big.bmp";  //include the file extension *.bmp SolidGrey.bmp, Bars.bmp, Burgard.bmp, usarmaze_big.bmp

        #region PROMRDS-based Maze Properties
        public string GroundTexture;
        public string[] WallTextures =
        {
            "Maze_Textures/SolidBlack.bmp",
            "Maze_Textures/SolidRed.bmp",
            "Maze_Textures/SolidLime.bmp",
            "Maze_Textures/SolidYellow.bmp",
            "Maze_Textures/SolidBlue.bmp",
            "Maze_Textures/SolidMagenta.bmp",
            "Maze_Textures/SolidCyan.bmp",
            "Maze_Textures/SolidWhite.bmp",
            "Maze_Textures/SolidDarkGrey.bmp",
            "Maze_Textures/SolidMaroon.bmp",
            "Maze_Textures/SolidGreen.bmp",
            "Maze_Textures/SolidOlive.bmp",
            "Maze_Textures/SolidNavy.bmp",
            "Maze_Textures/SolidPurple.bmp",
            "Maze_Textures/SolidCobalt.bmp",
            "Maze_Textures/SolidGrey.bmp"
        };
        public Vector3[] WallColors;
        public float[] HeightMap =
            {
                3.0f,
                5.0f,
                10.0f,
                8.0f,
                10.0f,
                15.0f,
                15.0f,
                20.0f,
                0.0f,
                5.0f,
                5.0f,
                5.0f,
                5.0f,
                5.0f,
                5.0f,
                5.0f,
            };
        public float[] MassMap =
            {
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
            };
        public bool[] UseSphere =
            {
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                false
            };
        public Vector3 CameraPosition = new Vector3(0, 0.5f, 0);
        public float CameraTiltAngle = 0;
        public float WallBoxSize = 0.98f;
        public float GridSpacing = 0.1f;
        public float HeightScale = 0.1f;
        public float DefaultHeight = 10.0f;
        public float SphereScale = 0.1f;
        public float BounceThreshold = 0;

        // TT - Increased to 16 in Oct 2006
        // Added MassMap and created a local copy of HeightMap
        // in case there were not enough items in the State.
        // Added a flag as well to create a sphere instead of a box.
        private string[] _WallTextures = new string[16];
        private Vector3[] _WallColors = new Vector3[16];
        private float[] _WallHeights = new float[16];
        private float[] _WallMasses = new float[16];
        private bool[] _UseSphere = new bool[16];

        static int WallCellColorThresh = -10000000;    // pixels less than this value will be counted as walls

        static bool OptimizeBlocks = true;  //can significantly reduce number of entities and thus increase frames per second
        int BlockCounter = 0;               //to count blocks after optimization

        //++ TT
        // Added for better use of bitmaps
        static bool UseBackgroundColor = true;
        static bool CenterMaze = true;

        // Simple colors
        //
        // Anyone remember CGA?
        // You don't have to list all the values in an enum,
        // but it looks pretty
        //
        public enum BasicColor : byte
        {
            Black = 0,
            Red = 1,
            Lime = 2,
            Yellow = 3,
            Blue = 4,
            Magenta = 5,
            Cyan = 6,
            White = 7,
            DarkGrey = 8,
            Maroon = 9,
            Green = 10,
            Olive = 11,
            Navy = 12,
            Purple = 13,
            Cobalt = 14,
            Grey = 15
        }
        // Wall Colors
        // These are specified using the Vector3 format for colors
        // The values below correspond to the bmp files above
        static public readonly Vector3[] DefaultColors =
        {
            new Vector3(  0,   0,   0),
            new Vector3(255,   0,   0),
            new Vector3(  0, 255,   0),
            new Vector3(255, 255,   0),
            new Vector3(  0,   0, 255),
            new Vector3(255,   0, 255),
            new Vector3(  0, 255, 255),
            new Vector3(255, 255, 255),
            new Vector3(128, 128, 128),
            new Vector3(128,   0,   0),
            new Vector3(  0, 128,   0),
            new Vector3(128, 128,   0),
            new Vector3(  0,   0, 128),
            new Vector3(128,   0, 128),
            new Vector3(  0, 128, 128),
            new Vector3(192, 192, 192)
        };
        #endregion/// <summary>
        #endregion

        /// <summary>
        /// Service state
        /// </summary>
        [ServiceState]
        SimJaguarsState _state = new SimJaguarsState();

        /// <summary>
        /// Main service port
        /// </summary>
        [ServicePort("/SimJaguars", AllowMultipleInstances = true)]
        SimJaguarsOperations _mainPort = new SimJaguarsOperations();

        [SubscriptionManagerPartner]
        submgr.SubscriptionManagerPort _submgrPort = new submgr.SubscriptionManagerPort();

        /// <summary>
        /// SimulationEngine partner
        /// </summary>
        [Partner("SimulationEngine", Contract = simengine.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        simengine.SimulationEnginePort _simulationEnginePort = new simengine.SimulationEnginePort();
        private SimulationEnginePort _notificationTarget = new SimulationEnginePort();
        private SimulationEnginePort _simEnginePort = SimulationEngine.GlobalInstancePort;

        //Vector Ports for reading robot services, there must be a set for each added robot.
        //They are created null because the code will generate service so initially there are no services to read,
        //Webcam has no notify because the frames are directly requested.
        webcam.WebCamOperations[] _webcamPort = new webcam.WebCamOperations[num_of_robots + 1];
        lrf.SickLRFOperations[] _lrfPort = new lrf.SickLRFOperations[num_of_robots + 1];
        lrf.SickLRFOperations[] _lrfNotify = new lrf.SickLRFOperations[num_of_robots + 1];          //Requires initialization


        /// <summary>
        /// Service constructor
        /// </summary>
        public SimJaguarsService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }

        /// <summary>
        /// Service start
        /// </summary>
        protected override void Start()
        {

            // 
            // Add service specific initialization here
            // 

            base.Start();
            simtime.Start();
            InitializePorts();
            _state = new SimJaguarsState();
            _state.Initialize();  // It creates the new Robot() instance for each robot in num_of_robots;
            
            #region Call for Simulated Entities
            InitializeMaze();
            // Orient sim camera view point
            SetupCamera();
            // Add objects (entities) in our simulated world
            PopulateWorld();
            // Add Jaguar (entities) in our simulated world
            AddJaguars();
            #endregion

            #region DSS Handlers
            MainPortInterleave.CombineWith(
                Arbiter.Interleave(
                new TeardownReceiverGroup(Arbiter.Receive<DsspDefaultDrop>(false, _mainPort, DefaultDropHandler)),
                new ExclusiveReceiverGroup(),
                new ConcurrentReceiverGroup(Arbiter.Receive<Get>(true, _mainPort, DefaultGetHandler),
                Arbiter.Receive<DsspDefaultLookup>(true, _mainPort, DefaultLookupHandler)
                ))
            );
            #endregion

            MainPortInterleave.CombineWith(
                new Interleave(
                    new TeardownReceiverGroup(),
                    new ExclusiveReceiverGroup(
                //Call for the subscriptions to the services related to the entity that has been inserted
                        Arbiter.Receive<InsertSimulationEntity>(true, _notificationTarget, InsertEntityNotificationHandler)
                    ),
                    new ConcurrentReceiverGroup()
                )
            );

            Activate(
                Arbiter.Receive<lrf.Replace>(false, _lrfNotify[1], LRFHandler_1),
                Arbiter.Receive<lrf.Replace>(false, _lrfNotify[2], LRFHandler_2),
                Arbiter.Receive<lrf.Replace>(false, _lrfNotify[3], LRFHandler_3)
            );

        }

        void InitializePorts()
        {
            //Starts in 1 because we are leaving 0 as null.
            for (int i = 1; i < num_of_robots + 1; i++)
            {
                _lrfNotify[i] = new lrf.SickLRFOperations();
            }
        }


        #region Simulation Entities

        #region Environment Entities
        public void State()
        {
            int i;
            // Initialize the wall colors
            WallColors = new Vector3[16];
            for (i = 0; i < 16; i++)
                WallColors[i] = new Vector3(DefaultColors[i].X,
                                        DefaultColors[i].Y,
                                        DefaultColors[i].Z);
        }
        private void SetupCamera()
        {
            // Set up initial view
            CameraView view = new CameraView();
            view.EyePosition = new Vector3(-12.52f, 22.5f, 11.51f);
            view.LookAtPoint = new Vector3(0f, -1f, -1.2f);
            SimulationEngine.GlobalInstancePort.Update(view);
        }
        private void PopulateWorld()
        {
            AddSky();
            AddGround();
            AddMaze();
            AddCameras();
        }
        private void AddCameras()
        {
            CameraEntity cam1 = new CameraEntity(640, 480);
            cam1.State.Name = "Isometric";
            cam1.State.Pose.Position = new Vector3(26f, 18f, 10f);
            cam1.State.Pose.Orientation = Quaternion.FromAxisAngle(-1.45f, 2.1f, 0, (float)(Math.PI / 6.0f));
            cam1.IsRealTimeCamera = false;

            CameraEntity cam2 = new CameraEntity(640, 480);
            cam2.State.Name = "Top";
            cam2.State.Pose.Position = new Vector3(0f, 20f, 0f);
            cam2.State.Pose.Orientation = Quaternion.FromAxisAngle(-1, 0, 0, (float)(Math.PI / 2.0f));

            cam2.IsRealTimeCamera = false;

            SimulationEngine.GlobalInstancePort.Insert(cam1);
            SimulationEngine.GlobalInstancePort.Insert(cam2);
        }
        void AddSky()
        {
            // Add a sky using a static texture. We will use the sky texture
            // to do per pixel lighting on each simulation visual entity
            SkyEntity sky = new SkyEntity("directions.dds", "sky_diff.dds");
            SimulationEngine.GlobalInstancePort.Insert(sky);

            // Add a directional light to simulate the sun.
            LightSourceEntity sun = new LightSourceEntity();
            sun.State.Name = "Sun";
            sun.Type = LightSourceEntityType.Directional;
            sun.Color = new Vector4(0.8f, 0.8f, 0.8f, 1);
            sun.Direction = new Vector3(-1.0f, -1.0f, 0.5f);
            SimulationEngine.GlobalInstancePort.Insert(sun);
        }
        void AddGround()
        {
            HeightFieldShapeProperties hf = new HeightFieldShapeProperties("height field",
                64, // number of rows
                100, // distance in meters, between rows
                64, // number of columns
                100, // distance in meters, between columns
                1, // scale factor to multiple height values
                -1000); // vertical extent of the height field. Should be set to large negative values

            // create array with height samples
            hf.HeightSamples = new HeightFieldSample[hf.RowCount * hf.ColumnCount];
            for (int i = 0; i < hf.RowCount * hf.ColumnCount; i++)
            {
                hf.HeightSamples[i] = new HeightFieldSample();
                hf.HeightSamples[i].Height = (short)(Math.Sin(i * 0.01));
            }

            // create a material for the entire field. We could also specify material per sample.
            hf.Material = new MaterialProperties("ground", 0.8f, 0.5f, 0.8f);

            // insert ground entity in simulation and specify a texture
            SimulationEngine.GlobalInstancePort.Insert(new HeightFieldEntity(hf, "grass.jpg"));
        }

        #region TT Maze Build-up
        void InitializeMaze()
        {
            int i;

            if (Mazefile == null)
                Mazefile = "ModelLarge.bmp";


            // Make sure that there is a floor texture
            // Plenty of others to try, e.g. concrete.jpg.
            if (GroundTexture == null || GroundTexture == "")
                GroundTexture = "cellfloor.jpg";

            State();
            // This is a little bit of insurance against a bad
            // config file ...
            // Copy as many textures as available up to the max
            for (i = 0; (i < 16) && (i < WallTextures.Length); i++)
            {
                _WallTextures[i] = WallTextures[i];
            }
            // Fill any remaining textures with empty string
            for (; i < 16; i++)
                _WallTextures[i] = "";

            // Copy as many colors as specified
            // NOTE: The constructor for the State sets all of the
            // colors to the standard ones, so any that are not
            // specified will default to them.
            for (i = 0; (i < 16) && (i < WallColors.Length); i++)
            {
                _WallColors[i] = WallColors[i];
            }
            // Fill any remaining colors with the defaults
            for (; i < 16; i++)
                _WallColors[i] = DefaultColors[i];

            // Copy as many heights as specified
            for (i = 0; (i < 16) && (i < HeightMap.Length); i++)
            {
                _WallHeights[i] = HeightMap[i];
            }
            // Fill any remaining heights with the defaults
            for (; i < 16; i++)
                _WallHeights[i] = 5.0f;

            // Copy as many weights as specified
            for (i = 0; (i < 16) && (i < MassMap.Length); i++)
            {
                _WallMasses[i] = MassMap[i];
            }
            // Fill any remaining weights with the defaults
            for (; i < 16; i++)
                _WallMasses[i] = 0.0f;

            // Copy as many sphere flags as specified
            for (i = 0; (i < 16) && (i < UseSphere.Length); i++)
            {
                _UseSphere[i] = UseSphere[i];
            }
            // Fill any remaining flags with false
            for (; i < 16; i++)
                _UseSphere[i] = false;

            if (SphereScale <= 0.0f)
                SphereScale = 1.0f;

            if (HeightScale <= 0.0f)
                HeightScale = 1.0f;

            // Copy back our private versions which might have the
            // effect of extending the state
            WallColors = _WallColors;
            WallTextures = _WallTextures;
            HeightMap = _WallHeights;
            MassMap = _WallMasses;
            UseSphere = _UseSphere;
        }

        // TT --
        // Converted the maze to a height array instead of booleans
        // Added an offset so that the maze can be centered in world coords
        // Added color for the walls using a texture, which is quite
        // flexible because it does not require recompiling the code.
        //
        // This is tacky -- Last minute change to get the correct
        // coordinates of the image color inside AddWall(). Should
        // be handled better ... Slap on the wrist!
        int xOffset, yOffset;

        void AddMaze()
        {
            // TT - Use a more sensible approach to handling the filename
            // If the path begins with slash or backslash, assume that it is
            // a full path from the MRDS root (or mountpoint).
            // If not, then look for the file in store\media\Maze_Textures
            string mazeFilename = Mazefile;
            if (mazeFilename.StartsWith("/") || mazeFilename.StartsWith("\\"))
                mazeFilename = LayoutPaths.RootDir.Substring(0, LayoutPaths.RootDir.Length - 1) + mazeFilename;
            else
                mazeFilename = LayoutPaths.RootDir + LayoutPaths.MediaDir + "Maze_Textures\\" + mazeFilename;
            // Display the resolved filename so the user can see it in case there is a problem
            Console.WriteLine("\nLoading Maze from: " + mazeFilename);

            // TT - float, not bool
            float[,] maze = ParseImage(mazeFilename);
            int height = maze.GetLength(1);
            int width = maze.GetLength(0);
            // TT
            int[,] counters = new int[width, height];
            int tempX1, tempY1;
            int tempX2, tempY2;
            BasicColor PixelColor;

            // TT -- Allow centering of the maze
            // (Easier to find in the simulator! However, you can't
            // as easily use the pixel coordinates to figure out where
            // things are.)
            if (CenterMaze)
            {
                xOffset = (int)((-width / 2.0) + 0.5);
                yOffset = (int)((-height / 2.0) + 0.5);
            }
            else
            {
                xOffset = 0;
                yOffset = 0;
            }

            if (OptimizeBlocks)
            {
                int count;
                int thisRowCount;
                float currentHeight;

                //merge horizontal blocks
                for (int y = 0; y < height; y++)
                {
                    int x = 0;
                    while (x < width - 1)
                    {
                        //at least 2 blocks to merge
                        if (maze[x, y] > 0.0f && maze[x + 1, y] > 0.0f)
                        {
                            int startX = x;
                            // TT -- Only merge wall segments of the same height
                            count = 0;
                            currentHeight = maze[x, y];
                            while (x < width && maze[x, y] > 0.0f
                                && maze[x, y] == currentHeight)
                            {
                                maze[x, y] = 0.0f;
                                counters[x, y] = 0;
                                x++;
                                count++;
                            }
                            // TT -- Just mark the map, don't draw anything here
                            counters[startX, y] = count;
                            maze[startX, y] = currentHeight;
                        }
                        else
                        {
                            if (maze[x, y] > 0.0f)
                                counters[x, y] = 1;
                            else
                                counters[x, y] = 0;
                            x++;
                        }
                    }
                    if (x < height)
                    {
                        if (maze[x, y] > 0.0f)
                            counters[x, y] = 1;
                        else
                            counters[x, y] = 0;
                    }
                }

                //merge remaining vertical blocks
                for (int x = 0; x < width; x++)
                {
                    int y = 0;
                    while (y < height - 1)
                    {
                        //at least 2 blocks to merge
                        // Must have the same row count AND height
                        if (counters[x, y] > 0 && counters[x, y + 1] == counters[x, y]
                            && maze[x, y] == maze[x, y + 1])
                        {
                            int startY = y;
                            count = 0;
                            thisRowCount = counters[x, y];
                            // TT -- Only merge wall segments of the same height
                            currentHeight = maze[x, y];
                            while (y < height && counters[x, y] == thisRowCount
                                && maze[x, y] == currentHeight)
                            {
                                maze[x, y] = 0.0f;
                                counters[x, y] = 0;
                                y++;
                                count++;
                            }
                            // TT -- Add offset
                            tempY1 = startY + yOffset;
                            tempX1 = x + xOffset;
                            tempY2 = startY + count - 1 + yOffset;
                            tempX2 = x + thisRowCount - 1 + xOffset;
                            PixelColor = ParseColor(img.GetPixel(x, startY));
                            AddWall(tempY1, tempX1, tempY2, tempX2, currentHeight, PixelColor);
                        }
                        else
                        {
                            if (counters[x, y] > 0)
                            {
                                tempY1 = y + yOffset;
                                tempX1 = x + xOffset;
                                tempY2 = y + yOffset;
                                tempX2 = x + counters[x, y] - 1 + xOffset;
                                PixelColor = ParseColor(img.GetPixel(x, y));
                                AddWall(tempY1, tempX1, tempY2, tempX2, maze[x, y], PixelColor);
                                maze[x, y] = 0.0f;
                                counters[x, y] = 0;
                            }

                            y++;
                        }
                    }
                    // TT -- Boundary condition
                    if (y < height)
                    {
                        if (counters[x, y] > 0)
                        {
                            tempY1 = y + yOffset;
                            tempX1 = x + xOffset;
                            tempY2 = y + yOffset;
                            tempX2 = x + counters[x, y] - 1 + xOffset;
                            PixelColor = ParseColor(img.GetPixel(x, y));
                            AddWall(tempY1, tempX1, tempY2, tempX2, maze[x, y], PixelColor);
                            maze[x, y] = 0.0f;
                            counters[x, y] = 0;
                        }
                    }
                }
            }

            //draw all blocks left over
            for (int y = 0; y < height; y++)
            {
                for (int x = 0; x < width; x++)
                {
                    if (maze[x, y] > 0.0f)
                    {
                        // TT -- Add offset
                        tempY1 = y + yOffset;
                        tempX1 = x + xOffset;
                        PixelColor = ParseColor(img.GetPixel(x, y));
                        AddWall(tempY1, tempX1, maze[x, y], PixelColor);
                        // This is only for debugging
                        // All blocks should be zero at the end ...
                        maze[x, y] = 0.0f;
                    }
                }
            }

            if (OptimizeBlocks)
            {
                Console.WriteLine("\nOptimization reduced number of wall blocks to: " + BlockCounter + "\n");
            }

        }

        // TT -- Simple fuzzy color parsing into the 16 basic colors
        const int BlackThreshold = 16;
        const int WhiteThreshold = 208;
        const int ColorThreshold = 128;
        const double GreyTolerance = 0.10;

        // This overload is not really necessary, but I'm lazy
        BasicColor ParseColor(Color pixel)
        {
            return ParseColor(pixel.R, pixel.G, pixel.B);
        }
        BasicColor ParseColor(int r, int g, int b)
        {
            int rgbSum = r + g + b;

            // Sort out Black and White straight away.
            // The assumption here is that sufficiently
            // intense or dark colors are just white or black.
            if (rgbSum < BlackThreshold * 3)
                return BasicColor.Black;
            else if (rgbSum > WhiteThreshold * 3)
                return BasicColor.White;
            else
            {
                // Next check for grey
                // This compares the range of values to see if they
                // are basically all equal, i.e. no dominant color = grey
                float normMax, normMin, normRange;
                int valMax, valMin;
                valMax = Math.Max(r, g);
                valMax = Math.Max(valMax, b);
                valMin = Math.Min(r, g);
                valMin = Math.Min(valMin, b);
                normMax = (float)valMax;
                normMin = (float)valMin;
                normMax /= rgbSum;
                normMin /= rgbSum;
                normRange = normMax - normMin;
                if (normRange < GreyTolerance)
                {
                    if (normMax >= 160)
                        return BasicColor.Grey;
                    else
                        return BasicColor.DarkGrey;
                }

                // Now we have a more complicated task
                // but it is made easier by the definition
                // of BasicColor
                byte color = 0;

                // Check for dark versions of the colors
                if (valMax < 160)
                {
                    color += 8;
                    if (r >= ColorThreshold / 2)
                        color += 1;
                    if (g >= ColorThreshold / 2)
                        color += 2;
                    if (b >= ColorThreshold / 2)
                        color += 4;
                }
                else
                {
                    // Now check the thresholds for normal colors
                    if (r >= ColorThreshold)
                        color += 1;
                    if (g >= ColorThreshold)
                        color += 2;
                    if (b >= ColorThreshold)
                        color += 4;
                }
                return (BasicColor)color;
            }
        }

        // Parse the image bitmap into a maze
        //
        // TT -- Modified so that you can have a background colour
        // to use for the bitmap and also to use the color as an
        // index for the height
        //
        static Bitmap img;
        float[,] ParseImage(string file)
        {
            img = (Bitmap)Image.FromFile(file);
            float[,] imgArray = new float[img.Width, img.Height];
            int WallCount = 0;

            // TT -- Allow background color to be used
            // Select the color of the top-left pixel.
            // We could be a lot smarter here. For instance, search the
            // image and find the predominant color which must be the
            // background. However, this will do for now.
            //bool IsWall;
            //BasicColor Background = BasicColor.White;
            Color BackgroundColor = Color.White;
            Color PixelColor;
            BasicColor PixelBasicColor;

            if (UseBackgroundColor)
            {
                BackgroundColor = img.GetPixel(0, 0);
                //                Background = ParseColor(BackgroundColor);
            }

            for (int y = 0; y < img.Height; y++)
            {
                for (int x = 0; x < img.Width; x++)
                {
                    if (UseBackgroundColor)
                    {
                        // Get a basic pixel color
                        PixelColor = img.GetPixel(x, y);
                        PixelBasicColor = ParseColor(PixelColor);
                        if (PixelColor.R != BackgroundColor.R ||
                            PixelColor.G != BackgroundColor.G ||
                            PixelColor.B != BackgroundColor.B)
                        {
                            // Return the height at this pixel location
                            imgArray[x, y] = _WallHeights[(byte)PixelBasicColor];
                            WallCount++;
                        }
                        else
                            imgArray[x, y] = 0.0f;
                    }
                    else
                    {
                        if (img.GetPixel(x, y).ToArgb() < WallCellColorThresh)
                        {
                            imgArray[x, y] = DefaultHeight;
                            WallCount++;
                        }
                        else
                            imgArray[x, y] = 0.0f;
                    }
                }
            }
            Console.WriteLine("\nAdding grid world of size " + img.Width + " x " + img.Height + ". With " + WallCount + " wall blocks.");
            return imgArray;
        }


        //TT -- Changed to include height and color
        //        void AddWall(int row, int col)
        //        {
        //            AddWall(row, col, DefaultHeight, BasicColor.White);
        //        }

        // Adds a simple cube at a specified location in the maze grid
        void AddWall(int row, int col, float height, BasicColor color)
        {

            // TT Oct-2006 - Add an option to use a sphere instead of a cube
            if (_UseSphere[(byte)color])
            {
                AddBall((float)(-row * GridSpacing), (float)(-col * GridSpacing), height, color);
            }
            else
            {
                // Dimensions are in meters
                Vector3 dimensions =
                    new Vector3(WallBoxSize * GridSpacing,
                            height * HeightScale,
                            WallBoxSize * GridSpacing);
                BoxShapeProperties cBoxShape = null;
                SingleShapeEntity box = null;

                // Create a simple box shape
                cBoxShape = new BoxShapeProperties(
                        _WallMasses[(byte)color], // mass in kilograms.
                        new Pose(),     // relative pose
                        dimensions);    // dimensions
                MaterialProperties bouncyMaterial = new MaterialProperties("Bouncy", 1.0f, 0.5f, 0.6f);
                //cBoxShape.Material = new MaterialProperties("gbox", 1.0f, 0.4f, 0.5f);
                cBoxShape.Material = bouncyMaterial;
                // Set the color of the box according to the bitmap image
                // or the specified color if no bitmap
                if (_WallTextures[(byte)color] == "")
                {
                    // TT - Changed for October CTP because DiffuseColor
                    // is a Vector4, but my WallColors are Vector3
                    //cBoxShape.DiffuseColor = _WallColors[(byte)color];
                    cBoxShape.DiffuseColor.X = (float)(_WallColors[(byte)color].X / 255.0);
                    cBoxShape.DiffuseColor.Y = (float)(_WallColors[(byte)color].Y / 255.0);
                    cBoxShape.DiffuseColor.Z = (float)(_WallColors[(byte)color].Z / 255.0);
                    cBoxShape.DiffuseColor.W = 0.5f;
                }
                else
                    cBoxShape.TextureFileName = _WallTextures[(byte)color];

                box = new SingleShapeEntity(new BoxShape(cBoxShape),
                    new Vector3(col * -GridSpacing,
                            height * HeightScale / 2,
                            -(row * GridSpacing)));

                // Name the entity. All entities must have unique names
                box.State.Name = "wall_" + row + "_" + col;

                // Insert entity in simulation.  
                SimulationEngine.GlobalInstancePort.Insert(box);
            }

            BlockCounter++;
        }

        // TT -- Changed to add a height and color
        //        void AddWall(int startRow, int startCol, int endRow, int endCol)
        //        {
        //            AddWall(startRow, startCol, endRow, endCol, DefaultHeight);
        //        }

        // Adds a long wall in the maze grid
        // Useful for reducing number of elements in simulation for better performance
        // TT -- Note that the existing code used height to refer to the
        // depth of the wall. Therefore the real height is called boxSize.
        void AddWall(int startRow, int startCol, int endRow, int endCol, float boxSize, BasicColor color)
        {
            int width = Math.Abs(endCol - startCol) + 1;
            int height = Math.Abs(endRow - startRow) + 1;

            float realWidth = (width * GridSpacing) - (GridSpacing - WallBoxSize * GridSpacing);
            float realHeight = (height * GridSpacing) - (GridSpacing - WallBoxSize * GridSpacing);

            //because the box is placed relative to the center of mass
            float widthOffset = (Math.Abs(endCol - startCol) * GridSpacing) / 2;
            float heightOffset = -(Math.Abs(endRow - startRow) * GridSpacing) / 2;

            if (_UseSphere[(byte)color])
            {
                AddBall((float)((-startRow * GridSpacing) + heightOffset),
                    (float)((-startCol * GridSpacing) - widthOffset),
                    (float)(Math.Sqrt(realWidth * realWidth + realHeight * realHeight)),
                    color);
            }
            else
            {
                // This object is a wall (stretched cube)
                Vector3 dimensions =
                    new Vector3(realWidth, boxSize * HeightScale, realHeight);
                // Dimensions are in meters
                BoxShapeProperties cBoxShape = null;
                SingleShapeEntity box = null;

                cBoxShape = new BoxShapeProperties(
                        _WallMasses[(byte)color], // mass in kilograms.
                        new Pose(),     // relative pose
                        dimensions);    // dimensions
                //                cBoxShape = new BoxShapeProperties(0, new Pose(), dimensions);
                // Walls have the same properties as the ground
                MaterialProperties bouncyMaterial = new MaterialProperties("Bouncy", 1.0f, 0.5f, 0.6f);
                //cBoxShape.Material = new MaterialProperties("gbox", 0.8f, 0.5f, 0.8f);
                cBoxShape.Material = bouncyMaterial;
                // Set the color of the box according to the bitmap image
                // or the specified color if no bitmap
                if (_WallTextures[(byte)color] == "")
                {
                    // TT - Changed for October CTP because DiffuseColor
                    // is a Vector4, but my WallColors are Vector3
                    //cBoxShape.DiffuseColor = _WallColors[(byte)color];
                    cBoxShape.DiffuseColor.X = (float)(_WallColors[(byte)color].X / 255.0);
                    cBoxShape.DiffuseColor.Y = (float)(_WallColors[(byte)color].Y / 255.0);
                    cBoxShape.DiffuseColor.Z = (float)(_WallColors[(byte)color].Z / 255.0);
                    cBoxShape.DiffuseColor.W = 0.5f;
                }
                else
                    cBoxShape.TextureFileName = _WallTextures[(byte)color];

                box = new SingleShapeEntity(new BoxShape(cBoxShape),
                    new Vector3((startCol * -GridSpacing) - widthOffset,
                                boxSize * HeightScale / 2,
                                -(startRow * GridSpacing) + heightOffset)
                    );
                // Name the entity. All entities must have unique names
                box.State.Name = "wall_" + startRow + "_" + startCol;
                SimulationEngine.GlobalInstancePort.Insert(box);
            }

            BlockCounter++;
        }

        void AddBall(float row, float col, float height, BasicColor color)
        {
            float radius;
            radius = SphereScale * height / 2.0f;

            MaterialProperties SlickMaterial = new MaterialProperties("Slick", 1.0f, 0.01f, 0.01f);
            //SphereShape ballShape = new SphereShape(new SphereShapeProperties(0.1f, new Pose(), 0.1f * _state.Scale));
            SphereShape ballShape = new SphereShape(
                //                new SphereShapeProperties(0.1f, new Pose(), radius));
                new SphereShapeProperties(_WallMasses[(byte)color], new Pose(), radius));
            ballShape.State.Material = SlickMaterial;
            //ballShape.State.DiffuseColor = new Vector4(1, 0, 0, 1);

            if (_WallTextures[(byte)color] == "")
            {
                ballShape.State.DiffuseColor.X = (float)(_WallColors[(byte)color].X / 255.0);
                ballShape.State.DiffuseColor.Y = (float)(_WallColors[(byte)color].Y / 255.0);
                ballShape.State.DiffuseColor.Z = (float)(_WallColors[(byte)color].Z / 255.0);
                ballShape.State.DiffuseColor.W = 1.0f;
            }
            else
                ballShape.State.TextureFileName = _WallTextures[(byte)color];

            SingleShapeEntity Ball = new SingleShapeEntity(
                ballShape,
                new Vector3());

            //Ball.State.Name = "Ball";
            // Name the entity. All entities must have unique names
            Ball.State.Name = "Ball" + (int)row + (int)col;
            //ballShape.State.Name = "ball_" + (int)row + "_" + (int)col;

            //Ball.State.Pose.Position = new Vector3(col, row, 0.5f);
            Ball.State.Pose.Position = new Vector3(col, radius, row);
            SimulationEngine.GlobalInstancePort.Insert(Ball);
        }

        #endregion

        #endregion

        #region Jaguar Creation
        public void AddJaguars()
        {
            for (int i = 1; i <= num_of_robots; i++)
            {
                //According to the num_of_robots we can decide which is the best pose
                CreateJaguar("Jaguar" + i, new Vector3(i - 2, 0.1f, 0));  
            }
        }
        /// <summary>
        /// Initialization constructor used when this entity is built programmatically
        /// </summary>
        /// <param name="initialPos"></param>
        public void CreateJaguar(string name, Vector3 initialPos)
        {
            JaguarEntity _jaguar = new JaguarEntity();
            _jaguar.State.Name = name;
            _jaguar.State.Pose.Position = initialPos;
            _jaguar.MotorTorqueScaling = 10.16f;

            CameraEntity frontCam = CreateCamera(name, initialPos);
            _jaguar.InsertEntity(frontCam);

            LaserRangeFinderEntity frontLaser = CreateLaser(name, initialPos);
            _jaguar.InsertEntity(frontLaser);

            //CameraSprite camSprite = new CameraSprite(0.5f, 0.5f, SpritePivotType.Center, 0, new Vector3(0.0f, 2.5f, 0.0f));
            //camSprite.State.Name = name + "_spriteCam";
            //camSprite.Flags = VisualEntityProperties.DisableBackfaceCulling;
            //camSprite.Flags = VisualEntityProperties.UsesAlphaBlending;
            //frontCam.InsertEntity(camSprite);

            // create a Jaguar
            SimulationEngine.GlobalInstancePort.Insert(_jaguar);

            Microsoft.Robotics.Simulation.Engine.EntitySubscribeRequestType esrt = new Microsoft.Robotics.Simulation.Engine.EntitySubscribeRequestType();
            esrt.Name = _jaguar.State.Name;
            Microsoft.Robotics.Simulation.Engine.SimulationEngine.GlobalInstancePort.Subscribe(esrt, _notificationTarget);
        }
        public CameraEntity CreateCamera(string name, Vector3 initialPos)
        {
            CameraEntity frontCam = new CameraEntity(640, 480, (float)(30.0 * Math.PI / 180.0));
            frontCam.CameraModel = CameraEntity.CameraModelType.AttachedChild;
            frontCam.State.Name = name + "_cam";
            frontCam.IsRealTimeCamera = true;    // update each frame
            frontCam.State.Pose.Position = new Vector3(
                0.04f,
                0.175f,
                -0.35f) + initialPos;

            simwebcam.Contract.CreateService(ConstructorPort, "http://localhost/" + frontCam.State.Name,
                Microsoft.Robotics.Simulation.Partners.CreateEntityPartner("http://localhost/" + frontCam.State.Name));

            return frontCam;
        }
        public LaserRangeFinderEntity CreateLaser(string name, Vector3 initialPos)
        {
            LaserRangeFinderEntity frontLaser = new LaserRangeFinderEntity(new Pose(new Vector3(-0.05f, 0.175f, -0.26f)));
            frontLaser.LaserBox.State.DiffuseColor = new Vector4(0.25f, 0.25f, 0.8f, 1.0f);
            frontLaser.State.Name = name + "_laser";
            frontLaser.LaserBox.BoxState.Dimensions = new Vector3(0.1f, 0.1f, 0.1f);

            simlrf.Contract.CreateService(ConstructorPort, "http://localhost/" + frontLaser.State.Name,
                Microsoft.Robotics.Simulation.Partners.CreateEntityPartner("http://localhost/" + frontLaser.State.Name));

            return frontLaser;
        }
        #endregion

        #endregion

        #region Handlers

        void InsertEntityNotificationHandler(InsertSimulationEntity ins)
        {
            //We create the robotID, which is only after "jaguar" (10 spaces) until the end of the string
            //If the name is changed in the AddJaguars() method, here there must be changed too to adjust.
            int robotID = Convert.ToInt32(ins.Body.State.Name.Substring(6, ins.Body.State.Name.Length - 6));

            //We map the inserted entity with the local variable for obtaining localization from simulator.
            _jaguar[robotID] = (JaguarEntity)ins.Body;

            //Time for ensuring services are created before attempting to subscribe.If a null error appears, then increase time.
            //System.Threading.Thread.Sleep(100);

            //Subscriptions
            _lrfPort[robotID] = ServiceForwarder<lrf.SickLRFOperations>(
                Microsoft.Dss.Core.Schemes.DsspTcp + "://" + localhost + ":" + tcpport + "/" + ins.Body.State.Name + "_laser" + "/sicklrf");
            _lrfPort[robotID].Subscribe(_lrfNotify[robotID]);
            _webcamPort[robotID] = ServiceForwarder<webcam.WebCamOperations>(
                 Microsoft.Dss.Core.Schemes.DsspTcp + "://" + localhost + ":" + tcpport + "/" + ins.Body.State.Name + "_cam" + "/webcamservice");
            //There is no webcamNotify because we handle webcam notifications by querying the service for frames.

            Console.ForegroundColor = ConsoleColor.Blue;
            Console.WriteLine(_jaguar[robotID].State.Name + " binded!");
            Console.ForegroundColor = ConsoleColor.Gray;
        }

        //Subscribers Response Port, just reads speeds for each side.
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public void UpdateSuccessMsgHandler(UpdateSuccessMsg update)
        {
            _jaguar[update.Body.robotID].SetVelocity((float)update.Body.LeftSpeed, (float)update.Body.RightSpeed);
        }

        #region DSS

        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public IEnumerator<ITask> SubscribeHandler(Subscribe subscribe)
        {
            SubscribeRequestType request = subscribe.Body;
            LogInfo("Subscribe request from: " + request.Subscriber);

            // Use the Subscription Manager to handle the subscribers
            yield return Arbiter.Choice(
                SubscribeHelper(_submgrPort, request, subscribe.ResponsePort),
                delegate(SuccessResult success)
                {
                    // Send a notification on successful subscription so that the
                    // subscriber can initialize its own state
                    base.SendNotificationToTarget<Replace>(request.Subscriber, _submgrPort, _state);
                },
                delegate(Exception e)
                {
                    LogError(null, "Subscribe failed", e);
                }
            );

            yield break;
        }
        #endregion

        #region Robots' Handlers
        /// <summary>
        /// ROBOT 1: Includes LRF and Webcam Handlers
        /// </summary>
        #region Robot 1
        ///Updates the state and maps it to the corresponding state variable
        void LRFHandler_1(lrf.Replace replace)
        {
            lrf.State laserData = GetMostRecentLaserNotification_1(replace.Body);
            //HERE WE CALL FOR THE AUTONOMOUS OPERATIONS, FOR NOW THESE ARE JUST TESTS...                                   
            if (laserData.DistanceMeasurements != null)
            {
                SpawnIterator(WebcamUpdate_1);
                UpdatePhysics(1);             
                _state.jaguar[1].Sensors.laserData = laserData;
                base.SendNotification<Replace>(_submgrPort, _state);
                //SafeWander(1);
                //WallFollow("left", 1);
                //Disperse(1);
                //Aggregate(1);
                //AvoidPast(1);
                //Seek(-7.5, 4.2, 1);
                //Seek(5,-5, 1);
                //Seek(-7, -7, 1);
                //UnitCenterDiamond(1);
                //UnitCenterWedge(1);
                //UnitCenterColumn(1);
                //UnitCenterLine(1);
                //Flock(1, "WEDGE");
                //FieldCover(1);
            }

            //Once we have the newest data, we skip the queued and reactivate the handler.
            GetMostRecentLaserNotification_1(laserData);
            Activate(Arbiter.Receive<lrf.Replace>(false, _lrfNotify[1], LRFHandler_1));
        }
        ///<Gets the most recent laser notification. Older notifications are dropped>
        private lrf.State GetMostRecentLaserNotification_1(lrf.State laserData)
        {
            lrf.Replace testReplace;

            // _laserNotify is a PortSet<>, P3 represents IPort<sicklrf.Replace> that
            // the portset contains
            int count = _lrfNotify[1].P3.ItemCount - 1;

            for (int i = 0; i < count; i++)
            {
                testReplace = _lrfNotify[1].Test<lrf.Replace>();
                if (testReplace.Body.TimeStamp > laserData.TimeStamp)
                {
                    laserData = testReplace.Body;
                }
            }

            if (count > 0)
            {
                Console.WriteLine(string.Format("Dropped {0} laser readings (laser start)", count));
                LogInfo(string.Format("Dropped {0} laser readings (laser start)", count));
            }
            return laserData;
        }
        ///Updates the webcam state and maps it to the corresponding state variables
        IEnumerator<ITask> WebcamUpdate_1()
        {
            yield return Arbiter.Choice(_webcamPort[1].QueryFrame(),
                success =>
                {
                    _state.jaguar[1].Sensors.WebcamData = success;
                    _state.jaguar[1].Sensors.WebcamBitmap = MakeBitmap(success.Size.Width, success.Size.Height, success.Frame);
                },
                failure =>
                {
                    LogError(failure.ToException());
                });
        }

        #endregion

        /// <summary>
        /// ROBOT 2: Includes LRF and Webcam Handlers
        /// </summary>
        #region Robot 2
        ///Updates the state and maps it to the corresponding state variable
        void LRFHandler_2(lrf.Replace replace)
        {
            lrf.State laserData = GetMostRecentLaserNotification_2(replace.Body);
            //HERE WE CALL FOR THE AUTONOMOUS OPERATIONS, FOR NOW THESE ARE JUST TESTS...
            if (laserData.DistanceMeasurements != null)
            {
                SpawnIterator(WebcamUpdate_2);
                UpdatePhysics(2);           
                _state.jaguar[2].Sensors.laserData = laserData;
                base.SendNotification<Replace>(_submgrPort, _state);
                //SafeWander(2);
                //Disperse(2);
                //Aggregate(2);
                //AvoidPast(2);
                //Seek(0, -1.3, 2);
                //Seek(12, -5, 2);
                //UnitCenterDiamond(2);
                //UnitCenterWedge(2);
                //UnitCenterColumn(2);
                //UnitCenterLine(2);
                //Flock(2, "WEDGE");
                //FieldCover(2);

            }

            //Once we have the newest data, we skip the queued and reactivate the handler.
            GetMostRecentLaserNotification_2(laserData);
            Activate(Arbiter.Receive<lrf.Replace>(false, _lrfNotify[2], LRFHandler_2));
        }
        ///<Gets the most recent laser notification. Older notifications are dropped>
        private lrf.State GetMostRecentLaserNotification_2(lrf.State laserData)
        {
            lrf.Replace testReplace;

            // _laserNotify is a PortSet<>, P3 represents IPort<sicklrf.Replace> that
            // the portset contains
            int count = _lrfNotify[2].P3.ItemCount - 1;

            for (int i = 0; i < count; i++)
            {
                testReplace = _lrfNotify[2].Test<lrf.Replace>();
                if (testReplace.Body.TimeStamp > laserData.TimeStamp)
                {
                    laserData = testReplace.Body;
                }
            }

            if (count > 0)
            {
                Console.WriteLine(string.Format("R2 Dropped {0} laser readings (laser start)", count));
                LogInfo(string.Format("R2 Dropped {0} laser readings (laser start)", count));
            }
            return laserData;
        }
        ///Updates the webcam state and maps it to the corresponding state variables
        IEnumerator<ITask> WebcamUpdate_2()
        {
            yield return Arbiter.Choice(_webcamPort[2].QueryFrame(),
                success =>
                {
                    _state.jaguar[2].Sensors.WebcamData = success;
                    _state.jaguar[2].Sensors.WebcamBitmap = MakeBitmap(success.Size.Width, success.Size.Height, success.Frame);
                },
                failure =>
                {
                    LogError(failure.ToException());
                });
        }
        #endregion

        /// <summary>
        /// ROBOT 3: Includes LRF and Webcam Handlers
        /// </summary>
        #region Robot 3
        ///Updates the sicklrf state and maps it to the corresponding state variable
        void LRFHandler_3(lrf.Replace replace)
        {
            lrf.State laserData = GetMostRecentLaserNotification_3(replace.Body);
            //HERE WE CALL FOR THE AUTONOMOUS OPERATIONS, FOR NOW THESE ARE JUST TESTS...                                    
            if (laserData.DistanceMeasurements != null)
            {
                UpdatePhysics(3);
                SpawnIterator(WebcamUpdate_3);
                _state.jaguar[3].Sensors.laserData = laserData;
                base.SendNotification<Replace>(_submgrPort, _state);
                //SafeWander(3);
                //AvoidPast(3);
                //WallFollow("right", 3);
                //Disperse(3);
                //Aggregate(3);
                //Seek(5, 5, 3);
                //Seek(16, -20, 3);
                //UnitCenterDiamond(3);
                //UnitCenterWedge(3);
                //UnitCenterColumn(3);
                //UnitCenterLine(3);
                //Flock(3, "WEDGE");
                //FieldCover(3);
            }

            //Once we have the newest data, we skip the queued and reactivate the handler.
            GetMostRecentLaserNotification_3(laserData);
            Activate(Arbiter.Receive<lrf.Replace>(false, _lrfNotify[3], LRFHandler_3));
        }
        ///<Gets the most recent laser notification. Older notifications are dropped>
        private lrf.State GetMostRecentLaserNotification_3(lrf.State laserData)
        {
            lrf.Replace testReplace;

            // _laserNotify is a PortSet<>, P3 represents IPort<sicklrf.Replace> that
            // the portset contains
            int count = _lrfNotify[3].P3.ItemCount - 1;

            for (int i = 0; i < count; i++)
            {
                testReplace = _lrfNotify[3].Test<lrf.Replace>();
                if (testReplace.Body.TimeStamp > laserData.TimeStamp)
                {
                    laserData = testReplace.Body;
                }
            }

            if (count > 0)
            {
                Console.WriteLine(string.Format("R3 Dropped {0} laser readings (laser start)", count));
                LogInfo(string.Format("R3 Dropped {0} laser readings (laser start)", count));
            }
            return laserData;
        }
        ///Updates the webcam state and maps it to the corresponding state variables
        IEnumerator<ITask> WebcamUpdate_3()
        {
            yield return Arbiter.Choice(_webcamPort[3].QueryFrame(),
                success =>
                {
                    _state.jaguar[3].Sensors.WebcamData = success;
                    _state.jaguar[3].Sensors.WebcamBitmap = MakeBitmap(success.Size.Width, success.Size.Height, success.Frame);
                },
                failure =>
                {
                    LogError(failure.ToException());
                });
        }
        #endregion

        void UpdatePhysics(int robotID)
        {
            if (_jaguar[robotID] != null)
            {
                _state.jaguar[robotID].Pose.Heading = _jaguar[robotID].CurrentHeading * 180 / Math.PI;
                _state.jaguar[robotID].Pose.Speed = ((Math.Sqrt(Math.Pow(_jaguar[robotID].State.Velocity.X, 2) +
                                                      Math.Pow(_jaguar[robotID].State.Velocity.Z, 2))));
                _state.jaguar[robotID].Pose.X_pose = _jaguar[robotID].State.Pose.Position.X;
                _state.jaguar[robotID].Pose.Y_pose = _jaguar[robotID].State.Pose.Position.Y;
                _state.jaguar[robotID].Pose.Z_pose = _jaguar[robotID].State.Pose.Position.Z;

                _state.jaguar[robotID].Pose.PositionVector = _jaguar[robotID].State.Pose.Position;
                _state.jaguar[robotID].Pose.VelocityVector = _jaguar[robotID].State.Velocity;
                _state.jaguar[robotID].Pose.OrientationVector = _jaguar[robotID].State.Pose.Orientation;

                _state.jaguar[robotID].Pose.LeftFrontSpeed = _jaguar[robotID].FrontLeftWheel.Wheel.AxleSpeed;
                _state.jaguar[robotID].Pose.LeftRearSpeed = _jaguar[robotID].RearLeftWheel.Wheel.AxleSpeed;
                _state.jaguar[robotID].Pose.RightFrontSpeed = _jaguar[robotID].FrontRightWheel.Wheel.AxleSpeed;
                _state.jaguar[robotID].Pose.RightRearSpeed = _jaguar[robotID].RearRightWheel.Wheel.AxleSpeed;
            }
        }
        System.Drawing.Bitmap MakeBitmap(int width, int height, byte[] imageData)
        {
            // NOTE: This code implicitly assumes that the width is a multiple
            // of four bytes because Bitmaps have to be longword aligned.
            // We really should look at bmp.Stride to see if there is any padding.
            // However, the width and height come from the webcam and most cameras
            // have resolutions that are multiples of four.
            Bitmap bmp = new Bitmap(width, height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
            System.Drawing.Imaging.BitmapData data = bmp.LockBits(
            new Rectangle(0, 0, bmp.Width, bmp.Height),
            System.Drawing.Imaging.ImageLockMode.WriteOnly,
            System.Drawing.Imaging.PixelFormat.Format24bppRgb
            );
            System.Runtime.InteropServices.Marshal.Copy(imageData, 0, data.Scan0, imageData.Length);
            bmp.UnlockBits(data);
            return bmp;
        }
        #endregion

        #endregion


    }

    #region Jaguar Model Constructors


    [DataContract]
    [DataMemberConstructor]
    public class JaguarEntity : VisualEntity
    {
        private static float InchesToMeters(float inches)
        {
            return (float)(inches * 2.54 / 100.0);
        }

        static float mass = 12.25f; // kg
        static float chassisClearance = 0.102f;
        static float wheelGap = 0.015f;
        static float wheelWidth = 0.080f;
        static float wheelDiameter = 0.274f; // m
        static float wheelMass = 1.815f; // kg

        static Vector3 chassisDimensions = new Vector3(
            0.32f,
            0.145f,
            0.46f);

        #region Wheel Properties and Location
        static Vector3 wheelFRPosition = new Vector3(
            chassisDimensions.X / 2.0f + wheelGap + wheelWidth / 2.0f,
            wheelDiameter / 2.0f,
            0.156f);
        static Vector3 wheelFLPosition = new Vector3(
            -wheelFRPosition.X,
            wheelFRPosition.Y,
            wheelFRPosition.Z);
        static Vector3 wheelRRPosition = new Vector3(
            wheelFRPosition.X,
            wheelFRPosition.Y,
            -0.156f);
        static Vector3 wheelRLPosition = new Vector3(
            -wheelFRPosition.X,
            wheelFRPosition.Y,
            -0.156f);

        // instance variables
        RotatingWheelEntity _wheelFR;
        RotatingWheelEntity _wheelFL;
        RotatingWheelEntity _wheelRR;
        RotatingWheelEntity _wheelRL;

        [Category("Wheels")]
        [DataMember]
        public RotatingWheelEntity FrontRightWheel
        {
            get { return _wheelFR; }
            set { _wheelFR = value; }
        }

        [Category("Wheels")]
        [DataMember]
        public RotatingWheelEntity FrontLeftWheel
        {
            get { return _wheelFL; }
            set { _wheelFL = value; }
        }

        [Category("Wheels")]
        [DataMember]
        public RotatingWheelEntity RearRightWheel
        {
            get { return _wheelRR; }
            set { _wheelRR = value; }
        }

        [Category("Wheels")]
        [DataMember]
        public RotatingWheelEntity RearLeftWheel
        {
            get { return _wheelRL; }
            set { _wheelRL = value; }
        }

        [Category("Wheels")]
        [DataMember]
        public int FrontRightEncoder
        {
            get { return (int)(_wheelFR.Rots * 600f); }
            set { _wheelFR.Rots = (float)value / 600f; }
        }

        [Category("Wheels")]
        [DataMember]
        public int FrontLeftEncoder
        {
            get { return (int)(_wheelFL.Rots * 600f); }
            set { _wheelFL.Rots = (float)value / 600f; }
        }

        [Category("Wheels")]
        [DataMember]
        public int RearRightEncoder
        {
            get { return (int)(_wheelRR.Rots * 600f); }
            set { _wheelRR.Rots = (float)value / 600f; }
        }

        [Category("Wheels")]
        [DataMember]
        public int RearLeftEncoder
        {
            get { return (int)(_wheelRL.Rots * 600f); }
            set { _wheelRL.Rots = (float)value / 600f; }
        }

        bool _isEnabled;
        /// <summary>
        /// True if drive mechanism is enabled
        /// </summary>
        [DataMember]
        [Description("True if the drive mechanism is enabled.")]
        public bool IsEnabled
        {
            get { return _isEnabled; }
            set { _isEnabled = value; }
        }

        float _motorTorqueScaling;
        /// <summary>
        /// Scaling factor to apply to motor torque requests
        /// </summary>
        [DataMember]
        [Description("Scaling factor to apply to motor torque requests.")]
        public float MotorTorqueScaling
        {
            get { return _motorTorqueScaling; }
            set { _motorTorqueScaling = value; }
        }
        #endregion

        /// <summary>
        /// Default constructor used when this entity is deserialized
        /// </summary>
        public JaguarEntity()
        {
        }


        public override void Initialize(xnagrfx.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            try
            {
                // chassis
                BoxShapeProperties chassisDesc = new BoxShapeProperties(
                    "chassis",
                    mass,
                    new Pose(new Vector3(0, chassisClearance + chassisDimensions.Y / 2.0f, 0)),
                    chassisDimensions);

                chassisDesc.Material = new MaterialProperties("chassisMaterial", 0.0f, 0.5f, 0.5f);

                BoxShape chassis = new BoxShape(chassisDesc);
                chassis.State.Name = "ChassisShape";
                base.State.PhysicsPrimitives.Add(chassis);
                //base.State.Flags = EntitySimulationModifiers.Kinematic;

                base.CreateAndInsertPhysicsEntity(physicsEngine);
                base.PhysicsEntity.SolverIterationCount = 128;

                // Wheels
                WheelShapeProperties wheelFRprop = new WheelShapeProperties("FrontRightWheel", wheelMass, wheelDiameter / 2.0f);
                WheelShapeProperties wheelFLprop = new WheelShapeProperties("FrontLeftWheel", wheelMass, wheelDiameter / 2.0f);
                WheelShapeProperties wheelRRprop = new WheelShapeProperties("RearRightWheel", wheelMass, wheelDiameter / 2.0f);
                WheelShapeProperties wheelRLprop = new WheelShapeProperties("RearLeftWheel", wheelMass, wheelDiameter / 2.0f);

                wheelFRprop.Flags |= WheelShapeBehavior.OverrideAxleSpeed;
                wheelFLprop.Flags |= WheelShapeBehavior.OverrideAxleSpeed;
                wheelRRprop.Flags |= WheelShapeBehavior.OverrideAxleSpeed;
                wheelRLprop.Flags |= WheelShapeBehavior.OverrideAxleSpeed;

                wheelFRprop.InnerRadius = 0.7f * wheelDiameter / 2.0f;
                wheelFLprop.InnerRadius = 0.7f * wheelDiameter / 2.0f;
                wheelRRprop.InnerRadius = 0.7f * wheelDiameter / 2.0f;
                wheelRLprop.InnerRadius = 0.7f * wheelDiameter / 2.0f;

                wheelFRprop.LocalPose = new Pose(wheelFRPosition);
                wheelFLprop.LocalPose = new Pose(wheelFLPosition);
                wheelRRprop.LocalPose = new Pose(wheelRRPosition);
                wheelRLprop.LocalPose = new Pose(wheelRLPosition);

                TireForceFunctionDescription LongitudalFunction = new TireForceFunctionDescription();
                LongitudalFunction.ExtremumSlip = 1.0f;
                LongitudalFunction.ExtremumValue = 0.02f;
                LongitudalFunction.AsymptoteSlip = 2.0f;
                LongitudalFunction.AsymptoteValue = 0.01f;
                LongitudalFunction.StiffnessFactor = 10000000.0f;

                TireForceFunctionDescription LateralFunction = new TireForceFunctionDescription();
                LateralFunction.ExtremumSlip = 1.0f;
                LateralFunction.ExtremumValue = 0.02f;
                LateralFunction.AsymptoteSlip = 2.0f;
                LateralFunction.AsymptoteValue = 0.01f;
                LateralFunction.StiffnessFactor = 1000000.0f;

                wheelFRprop.TireLongitudalForceFunction = LongitudalFunction;
                wheelFLprop.TireLongitudalForceFunction = LongitudalFunction;
                wheelRRprop.TireLongitudalForceFunction = LongitudalFunction;
                wheelRLprop.TireLongitudalForceFunction = LongitudalFunction;

                wheelFRprop.TireLateralForceFunction = LateralFunction;
                wheelFLprop.TireLateralForceFunction = LateralFunction;
                wheelRRprop.TireLateralForceFunction = LateralFunction;
                wheelRLprop.TireLateralForceFunction = LateralFunction;

                _wheelFR = new RotatingWheelEntity(wheelFRprop);
                _wheelFR.State.Name = base.State.Name + " FrontRightWheel";
                _wheelFR.Parent = this;
                _wheelFR.State.Assets.Mesh = "JaguarWheel.obj";
                _wheelFR.MeshRotation = new Vector3(0, 180, 0);   // flip the wheel mesh
                _wheelFR.Initialize(device, physicsEngine);

                _wheelFL = new RotatingWheelEntity(wheelFLprop);
                _wheelFL.State.Name = base.State.Name + " FrontLeftWheel";
                _wheelFL.Parent = this;
                _wheelFL.State.Assets.Mesh = "JaguarWheel.obj";
                _wheelFL.Initialize(device, physicsEngine);

                _wheelRR = new RotatingWheelEntity(wheelRRprop);
                _wheelRR.State.Name = base.State.Name + " RearRightWheel";
                _wheelRR.Parent = this;
                _wheelRR.State.Assets.Mesh = "JaguarWheel.obj";
                _wheelRR.MeshRotation = new Vector3(0, 180, 0);   // flip the wheel mesh
                _wheelRR.Initialize(device, physicsEngine);

                _wheelRL = new RotatingWheelEntity(wheelRLprop);
                _wheelRL.State.Name = base.State.Name + " RearLeftWheel";
                _wheelRL.Parent = this;
                _wheelRL.State.Assets.Mesh = "JaguarWheel.obj";
                _wheelRL.Initialize(device, physicsEngine);

                State.Assets.Mesh = "JaguarBody.obj";



                base.Initialize(device, physicsEngine);

                //SetMotorTorque(1, 1); // used to test the top speed of the entity
            }
            catch (Exception ex)
            {
                // clean up
                if (PhysicsEntity != null)
                    PhysicsEngine.DeleteEntity(PhysicsEntity);

                HasBeenInitialized = false;
                InitError = ex.ToString();
            }
        }

        const float SPEED_DELTA = 0.5f;
        //Pose startPose;
        //double totalTime = -1;
        public override void Update(FrameUpdate update)
        {
            // used to test the top speed of the entity
            //if (totalTime < 0)
            //{
            //    startPose = State.Pose;
            //    totalTime = update.ElapsedTime;
            //}
            //else
            //{
            //    double distance = Vector3.Length(State.Pose.Position - startPose.Position);
            //    double speed = distance / totalTime;
            //    totalTime += update.ElapsedTime;
            //    Console.WriteLine(speed.ToString());
            //}

            // update state for us and all the shapes that make up the rigid body
            PhysicsEntity.UpdateState(true);

            if (_distanceToTravel > 0)
            {
                // DriveDistance update
                double currentDistance = Vector3.Length(State.Pose.Position - _startPoseForDriveDistance.Position);
                if (currentDistance >= _distanceToTravel)
                {
                    _wheelFR.Wheel.AxleSpeed = 0;
                    _wheelFL.Wheel.AxleSpeed = 0;
                    _wheelRR.Wheel.AxleSpeed = 0;
                    _wheelRL.Wheel.AxleSpeed = 0;
                    _leftTargetVelocity = 0;
                    _rightTargetVelocity = 0;
                    _distanceToTravel = 0;
                    // now that we're finished, post a response
                    if (_driveDistancePort != null)
                    {
                        SuccessFailurePort tmp = _driveDistancePort;
                        _driveDistancePort = null;
                        tmp.Post(new SuccessResult());
                    }
                }
                else if ((DateTime.Now - _startTime).TotalSeconds > _timeoutSeconds)
                {
                    if (_driveDistancePort != null)
                    {
                        SuccessFailurePort tmp = _driveDistancePort;
                        _driveDistancePort = null;
                        tmp.Post(new Exception("Request timed out."));
                    }
                }
                else
                {
                    // need to drive further, check to see if we should slow down
                    if (progressPoints.Count >= averageKernel)
                    {
                        double distanceRemaining = _distanceToTravel - currentDistance;
                        double framesToCompletion = distanceRemaining * averageKernel / (currentDistance - progressPoints.Dequeue());
                        if (framesToCompletion < decelerateThreshold)
                        {
                            if (Math.Abs(_leftTargetVelocity) > 0.1)
                            {
                                _leftTargetVelocity *= 0.5f;
                                _rightTargetVelocity *= 0.5f;
                            }
                            progressPoints.Clear();
                        }
                    }
                    progressPoints.Enqueue(currentDistance);
                }
            }
            else if (_targetRotation != double.MaxValue)
            {
                // RotateDegrees update
                float currentHeading = CurrentHeading;
                double angleDelta = currentHeading - _previousHeading;
                while (angleDelta > Math.PI)
                    angleDelta -= twoPI;
                while (angleDelta <= -Math.PI)
                    angleDelta += twoPI;
                _currentRotation += angleDelta;
                _previousHeading = currentHeading;  // for next frame

                float angleError;
                if (_targetRotation < 0)
                    angleError = (float)(_currentRotation - _targetRotation);
                else
                    angleError = (float)(_targetRotation - _currentRotation);

                if (angleError < acceptableRotationError)
                {
                    // current heading is within acceptableError or has overshot
                    // end the rotation
                    _targetRotation = double.MaxValue;
                    _wheelFR.Wheel.AxleSpeed = 0;
                    _wheelFL.Wheel.AxleSpeed = 0;
                    _wheelRR.Wheel.AxleSpeed = 0;
                    _wheelRL.Wheel.AxleSpeed = 0;
                    _leftTargetVelocity = 0;
                    _rightTargetVelocity = 0;
                    // now that we're finished, post a response
                    if (_rotateDegreesPort != null)
                    {
                        SuccessFailurePort tmp = _rotateDegreesPort;
                        _rotateDegreesPort = null;
                        tmp.Post(new SuccessResult());
                    }
                }
                else if ((DateTime.Now - _startTime).TotalSeconds > _timeoutSeconds)
                {
                    if (_rotateDegreesPort != null)
                    {
                        SuccessFailurePort tmp = _rotateDegreesPort;
                        _rotateDegreesPort = null;
                        tmp.Post(new Exception("Request timed out."));
                    }
                }
                else
                {
                    if (angleDelta != 0)
                    {
                        // need to turn more, check to see if we should slow down
                        if (progressPoints.Count >= averageKernel)
                        {
                            double framesToCompletion = Math.Abs(angleError * averageKernel / (_currentRotation - progressPoints.Dequeue()));
                            if (framesToCompletion < decelerateThreshold)
                            {
                                if (Math.Abs(_leftTargetVelocity) > 0.1)
                                    _leftTargetVelocity *= 0.5f;

                                if (Math.Abs(_rightTargetVelocity) > 0.1)
                                    _rightTargetVelocity *= 0.5f;
                                progressPoints.Clear();
                            }
                        }
                        progressPoints.Enqueue(_currentRotation);
                    }
                }
            }

            float left = _wheelFL.Wheel.AxleSpeed + _leftTargetVelocity;
            float right = _wheelFR.Wheel.AxleSpeed + _rightTargetVelocity;

            if (Math.Abs(left) > 0.1)
            {
                if (left > 0)
                    _wheelFL.Wheel.AxleSpeed -= SPEED_DELTA;
                else
                    _wheelFL.Wheel.AxleSpeed += SPEED_DELTA;
            }

            if (Math.Abs(right) > 0.1)
            {
                if (right > 0)
                    _wheelFR.Wheel.AxleSpeed -= SPEED_DELTA;
                else
                    _wheelFR.Wheel.AxleSpeed += SPEED_DELTA;
            }

            // match the rear wheels with the front wheels
            _wheelRL.Wheel.AxleSpeed = _wheelFL.Wheel.AxleSpeed;
            _wheelRR.Wheel.AxleSpeed = _wheelFR.Wheel.AxleSpeed;

            // update entities in fields
            _wheelFL.Update(update);
            _wheelFR.Update(update);
            _wheelRL.Update(update);
            _wheelRR.Update(update);

            // sim engine will update children
            base.Update(update);
        }

        public override void Render(RenderMode renderMode, MatrixTransforms transforms, CameraEntity currentCamera)
        {
            base.Render(renderMode, transforms, currentCamera);
            _wheelFL.Render(renderMode, transforms, currentCamera);
            _wheelFR.Render(renderMode, transforms, currentCamera);
            _wheelRL.Render(renderMode, transforms, currentCamera);
            _wheelRR.Render(renderMode, transforms, currentCamera);
        }

        #region Motor Base Control

        Pose _startPoseForDriveDistance;
        double _distanceToTravel;
        SuccessFailurePort _driveDistancePort = null;

        /// <summary>
        /// Applies constant power to both wheels, driving the motor base for a fixed distance, in the current direction
        /// </summary>
        /// <param name="distance">Distance to travel, in meters</param>
        /// <param name="power">Normalized power (torque) value for both wheels</param>
        /// <param name="responsePort">A port to report the result of the request, success or exception</param>
        public void DriveDistance(float distance, float power, SuccessFailurePort responsePort)
        {
            // reset any drivedistance or rotate degrees commands that haven't completed
            ResetRotationAndDistance();

            // keep track of the response port for when we complete the request
            _driveDistancePort = responsePort;

            // handle negative distances
            if (distance < 0)
            {
                distance = -distance;
                power = -power;
            }
            _startPoseForDriveDistance = State.Pose;
            _distanceToTravel = distance;
            SetAxleVelocity(power * _motorTorqueScaling, power * _motorTorqueScaling);
            _startTime = DateTime.Now;
        }

        // DriveDistance and RotateDegrees variables
        Queue<double> progressPoints = new Queue<double>();
        const int averageKernel = 6;
        const int decelerateThreshold = 6;
        const float twoPI = (float)(2 * Math.PI);
        DateTime _startTime;

        // RotateDegrees variables
        double _targetRotation = double.MaxValue;
        double _currentRotation = 0;
        double _previousHeading = 0;
        const float acceptableRotationError = 0.005f;
        SuccessFailurePort _rotateDegreesPort = null;

        float _timeoutSeconds = 15.0f;

        /// <summary>
        /// The timeout for DriveDistance and RotateDegrees commands in seconds.
        /// </summary>
        [DataMember]
        public float TimeoutSeconds
        {
            get { return _timeoutSeconds; }
            set { _timeoutSeconds = value; }
        }

        /// <summary>
        /// Applies constant power to each wheel (but of inverse polarity), rotating the motor base 
        /// through the given rotation.
        /// </summary>
        /// <param name="degrees">Rotation around Y axis, in degrees.</param>
        /// <param name="power">Normalized power (torque) value for both wheels</param>
        /// <param name="responsePort">A port to report the result of the request, success or exception</param>
        public void RotateDegrees(float degrees, float power, SuccessFailurePort responsePort)
        {
            // reset any drivedistance or rotate degrees commands that haven't completed
            ResetRotationAndDistance();

            // keep track of the response port for when we complete the request
            _rotateDegreesPort = responsePort;

            _targetRotation = xna.MathHelper.ToRadians(degrees);
            _currentRotation = 0;
            _previousHeading = CurrentHeading;

            if (degrees < 0)
                SetAxleVelocity(power * _motorTorqueScaling, -power * _motorTorqueScaling);
            else
                SetAxleVelocity(-power * _motorTorqueScaling, power * _motorTorqueScaling);

            _startTime = DateTime.Now;
        }

        /// <summary>
        /// Current heading, in radians, of robot base
        /// </summary>
        public float CurrentHeading
        {
            get
            {
                // return the axis angle of the quaternion
                xna.Vector3 euler = UIMath.QuaternionToEuler(State.Pose.Orientation);
                return xna.MathHelper.ToRadians(euler.Y); // heading is the rotation about the Y axis.
            }
        }

        /// <summary>
        /// When a direct update to motor torque or wheel velocity occurs
        /// we abandon any current DriveDistance or RotateDegrees commands
        /// </summary>
        /// <summary>
        /// When a direct update to motor torque or wheel velocity occurs
        /// we abandon any current DriveDistance or RotateDegrees commands
        /// </summary>
        void ResetRotationAndDistance()
        {
            progressPoints.Clear();
            _distanceToTravel = 0;
            _targetRotation = double.MaxValue;
            if (_driveDistancePort != null)
            {
                _driveDistancePort.Post(new Exception("Request superceded prior to completion."));
                _driveDistancePort = null;
            }
            if (_rotateDegreesPort != null)
            {
                _rotateDegreesPort.Post(new Exception("Request superceded prior to completion."));
                _rotateDegreesPort = null;
            }
        }


        /// <summary>
        /// Sets motor torque on the active wheels
        /// </summary>
        /// <param name="leftWheel"></param>
        /// <param name="rightWheel"></param>
        public void SetMotorTorque(float leftWheel, float rightWheel)
        {
            ResetRotationAndDistance();
            SetAxleVelocity(leftWheel * _motorTorqueScaling, rightWheel * _motorTorqueScaling);
        }

        float _leftTargetVelocity;
        float _rightTargetVelocity;

        /// <summary>
        /// Sets angular velocity (radians/sec) on both wheels
        /// </summary>
        /// <param name="value"></param>
        public void SetVelocity(float value)
        {
            ResetRotationAndDistance();
            SetVelocity(value, value);
        }

        /// <summary>
        /// Sets angular velocity on the wheels
        /// </summary>
        /// <param name="left"></param>
        /// <param name="right"></param>
        public void SetVelocity(float left, float right)
        {
            ResetRotationAndDistance();
            if (_wheelFR == null || _wheelFL == null)
                return;

            left = ValidateWheelVelocity(left);
            right = ValidateWheelVelocity(right);

            // v is in m/sec - convert to an axle speed
            //  2Pi(V/2PiR) = V/R
            SetAxleVelocity(left / _wheelFR.Wheel.State.Radius,
                right / _wheelFL.Wheel.State.Radius);
        }

        private void SetAxleVelocity(float left, float right)
        {
            _leftTargetVelocity = left;
            _rightTargetVelocity = right;
        }

        const float MAX_VELOCITY = 20.0f;
        const float MIN_VELOCITY = -MAX_VELOCITY;

        float ValidateWheelVelocity(float value)
        {
            if (value > MAX_VELOCITY)
                return MAX_VELOCITY;
            if (value < MIN_VELOCITY)
                return MIN_VELOCITY;

            return value;
        }
        #endregion
    }


    [DataContract]
    public class RotatingWheelEntity : WheelEntity
    {
        const float Rotscale = (float)(-1.0 / (2.0 * Math.PI));
        public float Rots = 0;

        public RotatingWheelEntity()
        {
        }

        public RotatingWheelEntity(WheelShapeProperties wheelShape)
            : base(wheelShape)
        {
        }

        public override void Initialize(Microsoft.Xna.Framework.Graphics.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            base.Initialize(device, physicsEngine);
        }

        public override void Update(FrameUpdate update)
        {
            base.Update(update);

            // set the wheel to the current position
            Wheel.State.LocalPose.Orientation = TypeConversion.FromXNA(
                xna.Quaternion.CreateFromAxisAngle(new xna.Vector3(-1, 0, 0), (float)(Rots * 2 * Math.PI)));

            // update the Rots for the next frame
            Rots += (float)(Wheel.AxleSpeed * update.ElapsedTime * Rotscale);
        }
    }



    #endregion
}


